notebook.community

Edit and run 





In [1]:



    


from miscpy.utils.sympyhelpers import *
init_printing()
from sympy.utilities.codegen import codegen

Set up rotation matrices representing a 3-1-3 $(\psi,\theta,\phi)$ Euler angle set.





In [2]:



    


aCi = rotMat(3,psi)
cCa = rotMat(1,th)
bCc = rotMat(3,ph)
aCi,cCa,bCc



    


















    
Out[2]:







$$\left ( \left[\begin{matrix}\cos{\left (\psi \right )} & \sin{\left (\psi \right )} & 0\\- \sin{\left (\psi \right )} & \cos{\left (\psi \right )} & 0\\0 & 0 & 1\end{matrix}\right], \quad \left[\begin{matrix}1 & 0 & 0\\0 & \cos{\left (\theta \right )} & \sin{\left (\theta \right )}\\0 & - \sin{\left (\theta \right )} & \cos{\left (\theta \right )}\end{matrix}\right], \quad \left[\begin{matrix}\cos{\left (\phi \right )} & \sin{\left (\phi \right )} & 0\\- \sin{\left (\phi \right )} & \cos{\left (\phi \right )} & 0\\0 & 0 & 1\end{matrix}\right]\right )$$
















In [3]:



    


bCi = bCc*cCa*aCi; bCi #3-1-3 rotation



    


















    
Out[3]:







$$\left[\begin{matrix}- \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} & \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )} & \sin{\left (\phi \right )} \sin{\left (\theta \right )}\\- \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} & - \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} & \sin{\left (\theta \right )} \cos{\left (\phi \right )}\\\sin{\left (\psi \right )} \sin{\left (\theta \right )} & - \sin{\left (\theta \right )} \cos{\left (\psi \right )} & \cos{\left (\theta \right )}\end{matrix}\right]$$
















In [4]:



    


bCi_dot = difftotalmat(bCi,t,{th:thd,psi:psid,ph:phd});
bCi_dot



    


















    
Out[4]:







$$\left[\begin{matrix}- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \sin{\left (\theta \right )} & - \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\psi \right )} & \dot{\phi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )}\\\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} + \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} - \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\theta} \sin{\left (\psi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} & - \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\psi \right )} & - \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\\\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\psi \right )} \cos{\left (\theta \right )} & \dot{\psi} \sin{\left (\psi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\psi \right )} \cos{\left (\theta \right )} & - \dot{\theta} \sin{\left (\theta \right )}\end{matrix}\right]$$

$\tilde{\omega} = {}^\mathcal{B}C^{\mathcal{I}} {}^\mathcal{B}{\dot{C}}^{\mathcal{I}}$





In [5]:



    


omega_tilde = bCi*bCi_dot.T; omega_tilde



    


















    
Out[5]:







$$\left[\begin{matrix}\left(\dot{\phi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \sin{\left (\theta \right )}\right) + \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\psi \right )}\right) & \left(- \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) \left(\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} + \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} - \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\theta} \sin{\left (\psi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )}\right) + \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) & - \dot{\theta} \sin{\left (\phi \right )} \sin^{2}{\left (\theta \right )} + \left(\dot{\psi} \sin{\left (\psi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )}\right) + \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right)\\\left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\psi \right )}\right) + \left(- \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \sin{\left (\theta \right )}\right) + \left(\dot{\phi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\theta \right )} \cos{\left (\phi \right )} & \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) + \left(- \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \left(\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} + \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} - \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\theta} \sin{\left (\psi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )}\right) + \left(- \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\theta \right )} \cos{\left (\phi \right )} & - \dot{\theta} \sin^{2}{\left (\theta \right )} \cos{\left (\phi \right )} + \left(- \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\psi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) + \left(- \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\psi \right )} \cos{\left (\theta \right )}\right)\\\left(\dot{\phi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )}\right) \cos{\left (\theta \right )} - \left(- \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} + \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\psi \right )}\right) \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \sin{\left (\theta \right )}\right) \sin{\left (\psi \right )} \sin{\left (\theta \right )} & \left(- \dot{\phi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) \cos{\left (\theta \right )} + \left(\dot{\phi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} + \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\psi \right )} - \dot{\psi} \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \dot{\theta} \sin{\left (\psi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )}\right) \sin{\left (\psi \right )} \sin{\left (\theta \right )} - \left(- \dot{\phi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} - \dot{\phi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} - \dot{\psi} \sin{\left (\phi \right )} \cos{\left (\psi \right )} - \dot{\psi} \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) \sin{\left (\theta \right )} \cos{\left (\psi \right )} & - \dot{\theta} \sin{\left (\theta \right )} \cos{\left (\theta \right )} - \left(\dot{\psi} \sin{\left (\psi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\psi \right )} + \dot{\theta} \sin{\left (\psi \right )} \cos{\left (\theta \right )}\right) \sin{\left (\psi \right )} \sin{\left (\theta \right )}\end{matrix}\right]$$

$\left[{}^\mathcal{I}\boldsymbol{\omega}^{\mathcal{B}}\right]_\mathcal{B} = \left[ {}^\mathcal{B}C^{\mathcal{I}}_{32} \quad {}^\mathcal{B}C^{\mathcal{I}}_{13} \quad {}^\mathcal{B}C^{\mathcal{I}}_{21} \right]$





In [6]:



    


omega = simplify(Matrix([omega_tilde[2,1],omega_tilde[0,2],omega_tilde[1,0]]))
omega



    


















    
Out[6]:







$$\left[\begin{matrix}\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\\\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\\\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\end{matrix}\right]$$
















In [7]:



    


w1,w2,w3 = symbols('omega_1,omega_2,omega_3')



    











In [8]:



    


s0 = solve(omega - Matrix([w1,w2,w3]),[psid,thd,phd]); s0



    


















    
Out[8]:







$$\left \{ \dot{\phi} : - \frac{\omega_{1} \sin{\left (\phi \right )}}{\tan{\left (\theta \right )}} - \frac{\omega_{2} \cos{\left (\phi \right )}}{\tan{\left (\theta \right )}} + \omega_{3}, \quad \dot{\psi} : \frac{\omega_{1} \sin{\left (\phi \right )} + \omega_{2} \cos{\left (\phi \right )}}{\sin{\left (\theta \right )}}, \quad \dot{\theta} : \omega_{1} \cos{\left (\phi \right )} - \omega_{2} \sin{\left (\phi \right )}\right \}$$

Find EOM (second derivatives of Euler Angles)





In [24]:



    


I1,I2,I3 = symbols("I_1,I_2,I_3",real=True,positive=True)
iWb_B = omega
I_G_B = diag(I1,I2,I3)
I_G_B



    


















    
Out[24]:







$$\left[\begin{matrix}I_{1} & 0 & 0\\0 & I_{2} & 0\\0 & 0 & I_{3}\end{matrix}\right]$$
















In [10]:



    


diffmap = {th:thd,psi:psid,ph:phd,thd:thdd,psid:psidd,phd:phdd}
diffmap



    


















    
Out[10]:







$$\left \{ \phi : \dot{\phi}, \quad \dot{\phi} : \ddot{\phi}, \quad \psi : \dot{\psi}, \quad \dot{\psi} : \ddot{\psi}, \quad \theta : \dot{\theta}, \quad \dot{\theta} : \ddot{\theta}\right \}$$
















In [11]:



    


t1 = I_G_B*difftotalmat(iWb_B,t,diffmap) 
t2 = skew(iWb_B)*I_G_B*iWb_B
t1,t2



    


















    
Out[11]:







$$\left ( \left[\begin{matrix}I_{1} \left(\dot{\phi} \dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\phi} \dot{\theta} \sin{\left (\phi \right )} + \ddot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\psi} \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )} + \ddot{\theta} \cos{\left (\phi \right )}\right)\\I_{2} \left(- \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\phi} \dot{\theta} \cos{\left (\phi \right )} + \ddot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\psi} \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \ddot{\theta} \sin{\left (\phi \right )}\right)\\I_{3} \left(\ddot{\phi} + \ddot{\psi} \cos{\left (\theta \right )} - \dot{\psi} \dot{\theta} \sin{\left (\theta \right )}\right)\end{matrix}\right], \quad \left[\begin{matrix}I_{2} \left(- \dot{\phi} - \dot{\psi} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\right) + I_{3} \left(\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\right)\\I_{1} \left(\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\right) + I_{3} \left(\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\right) \left(- \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\phi \right )}\right)\\I_{1} \left(\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\right) \left(- \dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )}\right) + I_{2} \left(\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\right)\end{matrix}\right]\right )$$
















In [12]:



    


dh_G_B = t1+t2
dh_G_B



    


















    
Out[12]:







$$\left[\begin{matrix}I_{1} \left(\dot{\phi} \dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\phi} \dot{\theta} \sin{\left (\phi \right )} + \ddot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\psi} \dot{\theta} \sin{\left (\phi \right )} \cos{\left (\theta \right )} + \ddot{\theta} \cos{\left (\phi \right )}\right) + I_{2} \left(- \dot{\phi} - \dot{\psi} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\right) + I_{3} \left(\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\right)\\I_{1} \left(\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\right) \left(\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\right) + I_{2} \left(- \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\phi} \dot{\theta} \cos{\left (\phi \right )} + \ddot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\psi} \dot{\theta} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - \ddot{\theta} \sin{\left (\phi \right )}\right) + I_{3} \left(\dot{\phi} + \dot{\psi} \cos{\left (\theta \right )}\right) \left(- \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} - \dot{\theta} \cos{\left (\phi \right )}\right)\\I_{1} \left(\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\right) \left(- \dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + \dot{\theta} \sin{\left (\phi \right )}\right) + I_{2} \left(\dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + \dot{\theta} \cos{\left (\phi \right )}\right) \left(\dot{\psi} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - \dot{\theta} \sin{\left (\phi \right )}\right) + I_{3} \left(\ddot{\phi} + \ddot{\psi} \cos{\left (\theta \right )} - \dot{\psi} \dot{\theta} \sin{\left (\theta \right )}\right)\end{matrix}\right]$$
















In [13]:



    


t3 = expand(dh_G_B[0]*cos(ph)*I2 - dh_G_B[1]*sin(ph)*I1)



    











In [14]:



    


sol_thdd = simplify(solve(t3,thdd)) 
sol_thdd



    


















    
Out[14]:







$$\left [ \frac{I_{1}^{2} \dot{\phi} \dot{\psi} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} + \frac{I_{1}^{2} \dot{\phi} \dot{\theta} \sin{\left (2 \phi \right )}}{2} + I_{1}^{2} \dot{\psi}^{2} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\theta \right )} + \frac{I_{1}^{2} \dot{\psi} \dot{\theta} \left(\sin{\left (2 \phi - \theta \right )} + \sin{\left (2 \phi + \theta \right )}\right)}{4} - I_{1} I_{2} \dot{\phi} \dot{\psi} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} - I_{1} I_{2} \dot{\phi} \dot{\psi} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} - I_{1} I_{3} \dot{\phi} \dot{\psi} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} - \frac{I_{1} I_{3} \dot{\phi} \dot{\theta} \sin{\left (2 \phi \right )}}{2} - I_{1} I_{3} \dot{\psi}^{2} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\theta \right )} - \frac{I_{1} I_{3} \dot{\psi} \dot{\theta} \left(\sin{\left (2 \phi - \theta \right )} + \sin{\left (2 \phi + \theta \right )}\right)}{4} + I_{2}^{2} \dot{\phi} \dot{\psi} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} - \frac{I_{2}^{2} \dot{\phi} \dot{\theta} \sin{\left (2 \phi \right )}}{2} + I_{2}^{2} \dot{\psi}^{2} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} - \frac{I_{2}^{2} \dot{\psi} \dot{\theta} \left(\sin{\left (2 \phi - \theta \right )} + \sin{\left (2 \phi + \theta \right )}\right)}{4} - I_{2} I_{3} \dot{\phi} \dot{\psi} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} + \frac{I_{2} I_{3} \dot{\phi} \dot{\theta} \sin{\left (2 \phi \right )}}{2} - I_{2} I_{3} \dot{\psi}^{2} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} + \frac{I_{2} I_{3} \dot{\psi} \dot{\theta} \left(\sin{\left (2 \phi - \theta \right )} + \sin{\left (2 \phi + \theta \right )}\right)}{4}}{I_{1} I_{2}}\right ]$$
















In [15]:



    


t4= expand(dh_G_B[0]*sin(ph)*I2 + dh_G_B[1]*cos(ph)*I1)
t4



    


















    
Out[15]:







$$I_{1}^{2} \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + I_{1}^{2} \dot{\phi} \dot{\theta} \cos^{2}{\left (\phi \right )} + I_{1}^{2} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} + I_{1}^{2} \dot{\psi} \dot{\theta} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} - I_{1} I_{2} \dot{\phi} \dot{\theta} \sin^{2}{\left (\phi \right )} - I_{1} I_{2} \dot{\phi} \dot{\theta} \cos^{2}{\left (\phi \right )} + I_{1} I_{2} \ddot{\psi} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} + I_{1} I_{2} \ddot{\psi} \sin{\left (\theta \right )} \cos^{2}{\left (\phi \right )} + I_{1} I_{2} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \cos{\left (\theta \right )} + I_{1} I_{2} \dot{\psi} \dot{\theta} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} - I_{1} I_{3} \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - I_{1} I_{3} \dot{\phi} \dot{\theta} \cos^{2}{\left (\phi \right )} - I_{1} I_{3} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - I_{1} I_{3} \dot{\psi} \dot{\theta} \cos^{2}{\left (\phi \right )} \cos{\left (\theta \right )} - I_{2}^{2} \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + I_{2}^{2} \dot{\phi} \dot{\theta} \sin^{2}{\left (\phi \right )} - I_{2}^{2} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} + I_{2}^{2} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \cos{\left (\theta \right )} + I_{2} I_{3} \dot{\phi} \dot{\psi} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} - I_{2} I_{3} \dot{\phi} \dot{\theta} \sin^{2}{\left (\phi \right )} + I_{2} I_{3} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin{\left (\theta \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )} - I_{2} I_{3} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \cos{\left (\theta \right )}$$
















In [16]:



    


sol_psidd = simplify(solve(t4,psidd)) 
sol_psidd



    


















    
Out[16]:







$$\left [ \frac{- \frac{I_{1}^{2} \dot{\phi} \dot{\psi} \cos{\left (2 \phi - \theta \right )}}{4} + \frac{I_{1}^{2} \dot{\phi} \dot{\psi} \cos{\left (2 \phi + \theta \right )}}{4} - \frac{I_{1}^{2} \dot{\phi} \dot{\theta} \cos{\left (2 \phi \right )}}{2} - \frac{I_{1}^{2} \dot{\phi} \dot{\theta}}{2} - \frac{I_{1}^{2} \dot{\psi}^{2} \cos{\left (2 \phi - 2 \theta \right )}}{8} + \frac{I_{1}^{2} \dot{\psi}^{2} \cos{\left (2 \phi + 2 \theta \right )}}{8} - \frac{I_{1}^{2} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}}{2} - \frac{I_{1}^{2} \dot{\psi} \dot{\theta} \cos{\left (2 \phi - \theta \right )}}{4} - \frac{I_{1}^{2} \dot{\psi} \dot{\theta} \cos{\left (2 \phi + \theta \right )}}{4} + I_{1} I_{2} \dot{\phi} \dot{\theta} - I_{1} I_{2} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )} + \frac{I_{1} I_{3} \dot{\phi} \dot{\psi} \cos{\left (2 \phi - \theta \right )}}{4} - \frac{I_{1} I_{3} \dot{\phi} \dot{\psi} \cos{\left (2 \phi + \theta \right )}}{4} + \frac{I_{1} I_{3} \dot{\phi} \dot{\theta} \cos{\left (2 \phi \right )}}{2} + \frac{I_{1} I_{3} \dot{\phi} \dot{\theta}}{2} + \frac{I_{1} I_{3} \dot{\psi}^{2} \cos{\left (2 \phi - 2 \theta \right )}}{8} - \frac{I_{1} I_{3} \dot{\psi}^{2} \cos{\left (2 \phi + 2 \theta \right )}}{8} + \frac{I_{1} I_{3} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}}{2} + \frac{I_{1} I_{3} \dot{\psi} \dot{\theta} \cos{\left (2 \phi - \theta \right )}}{4} + \frac{I_{1} I_{3} \dot{\psi} \dot{\theta} \cos{\left (2 \phi + \theta \right )}}{4} + \frac{I_{2}^{2} \dot{\phi} \dot{\psi} \cos{\left (2 \phi - \theta \right )}}{4} - \frac{I_{2}^{2} \dot{\phi} \dot{\psi} \cos{\left (2 \phi + \theta \right )}}{4} + \frac{I_{2}^{2} \dot{\phi} \dot{\theta} \cos{\left (2 \phi \right )}}{2} - \frac{I_{2}^{2} \dot{\phi} \dot{\theta}}{2} + \frac{I_{2}^{2} \dot{\psi}^{2} \cos{\left (2 \phi - 2 \theta \right )}}{8} - \frac{I_{2}^{2} \dot{\psi}^{2} \cos{\left (2 \phi + 2 \theta \right )}}{8} - \frac{I_{2}^{2} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}}{2} + \frac{I_{2}^{2} \dot{\psi} \dot{\theta} \cos{\left (2 \phi - \theta \right )}}{4} + \frac{I_{2}^{2} \dot{\psi} \dot{\theta} \cos{\left (2 \phi + \theta \right )}}{4} - \frac{I_{2} I_{3} \dot{\phi} \dot{\psi} \cos{\left (2 \phi - \theta \right )}}{4} + \frac{I_{2} I_{3} \dot{\phi} \dot{\psi} \cos{\left (2 \phi + \theta \right )}}{4} - \frac{I_{2} I_{3} \dot{\phi} \dot{\theta} \cos{\left (2 \phi \right )}}{2} + \frac{I_{2} I_{3} \dot{\phi} \dot{\theta}}{2} - \frac{I_{2} I_{3} \dot{\psi}^{2} \cos{\left (2 \phi - 2 \theta \right )}}{8} + \frac{I_{2} I_{3} \dot{\psi}^{2} \cos{\left (2 \phi + 2 \theta \right )}}{8} + \frac{I_{2} I_{3} \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}}{2} - \frac{I_{2} I_{3} \dot{\psi} \dot{\theta} \cos{\left (2 \phi - \theta \right )}}{4} - \frac{I_{2} I_{3} \dot{\psi} \dot{\theta} \cos{\left (2 \phi + \theta \right )}}{4}}{I_{1} I_{2} \sin{\left (\theta \right )}}\right ]$$
















In [17]:



    


sol_phdd = solve(dh_G_B[2],phdd)
sol_phdd



    


















    
Out[17]:







$$\left [ \frac{I_{1} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin^{2}{\left (\theta \right )} \cos{\left (\phi \right )} - 2 I_{1} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} + I_{1} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )} - I_{1} \dot{\theta}^{2} \sin{\left (\phi \right )} \cos{\left (\phi \right )} - I_{2} \dot{\psi}^{2} \sin{\left (\phi \right )} \sin^{2}{\left (\theta \right )} \cos{\left (\phi \right )} + 2 I_{2} \dot{\psi} \dot{\theta} \sin^{2}{\left (\phi \right )} \sin{\left (\theta \right )} - I_{2} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )} + I_{2} \dot{\theta}^{2} \sin{\left (\phi \right )} \cos{\left (\phi \right )} - I_{3} \ddot{\psi} \cos{\left (\theta \right )} + I_{3} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )}}{I_{3}}\right ]$$
















In [ ]:

Find initial orientation such that $\mathbf h$ is down-pointing





In [25]:



    


h = sqrt(((I_G_B*Matrix([w1,w2,w3])).transpose()*(I_G_B*Matrix([w1,w2,w3])))[0]);h



    


















    
Out[25]:







$$\sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}$$
















In [28]:



    


eqs1 = simplify(bCi.transpose()*I_G_B*Matrix([w1,w2,w3]) - Matrix([0,0,-h])); eqs1 #equal 0



    


















    
Out[28]:







$$\left[\begin{matrix}- I_{1} \omega_{1} \left(\sin{\left (\phi \right )} \sin{\left (\psi \right )} \cos{\left (\theta \right )} - \cos{\left (\phi \right )} \cos{\left (\psi \right )}\right) - I_{2} \omega_{2} \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )} \cos{\left (\theta \right )}\right) + I_{3} \omega_{3} \sin{\left (\psi \right )} \sin{\left (\theta \right )}\\I_{1} \omega_{1} \left(\sin{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )} + \sin{\left (\psi \right )} \cos{\left (\phi \right )}\right) - I_{2} \omega_{2} \left(\sin{\left (\phi \right )} \sin{\left (\psi \right )} - \cos{\left (\phi \right )} \cos{\left (\psi \right )} \cos{\left (\theta \right )}\right) - I_{3} \omega_{3} \sin{\left (\theta \right )} \cos{\left (\psi \right )}\\I_{1} \omega_{1} \sin{\left (\phi \right )} \sin{\left (\theta \right )} + I_{2} \omega_{2} \sin{\left (\theta \right )} \cos{\left (\phi \right )} + I_{3} \omega_{3} \cos{\left (\theta \right )} + \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}\end{matrix}\right]$$
















In [34]:



    


simplify(solve(simplify(eqs1[0]*cos(psi) + eqs1[1]*sin(psi)),ph)) #phi solution



    


















    
Out[34]:







$$\left [ - 2 \operatorname{atan}{\left (\frac{I_{2} \omega_{2} - \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2}}}{I_{1} \omega_{1}} \right )}, \quad - 2 \operatorname{atan}{\left (\frac{I_{2} \omega_{2} + \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2}}}{I_{1} \omega_{1}} \right )}\right ]$$
















In [38]:



    


solve(simplify(expand(simplify(-eqs1[0]*sin(psi) + eqs1[1]*cos(psi)).subs(ph,atan(I1*w1/I2/w2)))),th) #th solution



    


















    
Out[38]:







$$\left [ - 2 \operatorname{atan}{\left (\frac{I_{3} \omega_{3} - \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}}{\omega_{2} \sqrt{\frac{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2}}{\omega_{2}^{2}}}} \right )}, \quad - 2 \operatorname{atan}{\left (\frac{I_{3} \omega_{3} + \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}}{\omega_{2} \sqrt{\frac{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2}}{\omega_{2}^{2}}}} \right )}\right ]$$
















In [36]:



    


simplify(eqs1[2].subs(ph,atan(I1*w1/I2/w2)))



    


















    
Out[36]:







$$I_{3} \omega_{3} \cos{\left (\theta \right )} + \omega_{2} \sqrt{\frac{I_{1}^{2} \omega_{1}^{2}}{\omega_{2}^{2}} + I_{2}^{2}} \sin{\left (\theta \right )} + \sqrt{I_{1}^{2} \omega_{1}^{2} + I_{2}^{2} \omega_{2}^{2} + I_{3}^{2} \omega_{3}^{2}}$$
















In [ ]:



    






    











In [ ]:

Generate MATLAB Code





In [18]:



    


out = codegen(("eom1",sol_psidd[0]), 'Octave', argument_sequence=[th,thd,psi,psid,ph,phd,I1,I2,I3]);out



    


















    
Out[18]:








[('eom1.m',
  "function out1 = eom1(theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = (-I_1.^2.*phidot.*psidot.*cos(2*phi - theta)/4 + I_1.^2.*phidot.*psidot.*cos(2*phi + theta)/4 - I_1.^2.*phidot.*thetadot.*cos(2*phi)/2 - I_1.^2.*phidot.*thetadot/2 - I_1.^2.*psidot.^2.*cos(2*phi - 2*theta)/8 + I_1.^2.*psidot.^2.*cos(2*phi + 2*theta)/8 - I_1.^2.*psidot.*thetadot.*cos(theta)/2 - I_1.^2.*psidot.*thetadot.*cos(2*phi - theta)/4 - I_1.^2.*psidot.*thetadot.*cos(2*phi + theta)/4 + I_1.*I_2.*phidot.*thetadot - I_1.*I_2.*psidot.*thetadot.*cos(theta) + I_1.*I_3.*phidot.*psidot.*cos(2*phi - theta)/4 - I_1.*I_3.*phidot.*psidot.*cos(2*phi + theta)/4 + I_1.*I_3.*phidot.*thetadot.*cos(2*phi)/2 + I_1.*I_3.*phidot.*thetadot/2 + I_1.*I_3.*psidot.^2.*cos(2*phi - 2*theta)/8 - I_1.*I_3.*psidot.^2.*cos(2*phi + 2*theta)/8 + I_1.*I_3.*psidot.*thetadot.*cos(theta)/2 + I_1.*I_3.*psidot.*thetadot.*cos(2*phi - theta)/4 + I_1.*I_3.*psidot.*thetadot.*cos(2*phi + theta)/4 + I_2.^2.*phidot.*psidot.*cos(2*phi - theta)/4 - I_2.^2.*phidot.*psidot.*cos(2*phi + theta)/4 + I_2.^2.*phidot.*thetadot.*cos(2*phi)/2 - I_2.^2.*phidot.*thetadot/2 + I_2.^2.*psidot.^2.*cos(2*phi - 2*theta)/8 - I_2.^2.*psidot.^2.*cos(2*phi + 2*theta)/8 - I_2.^2.*psidot.*thetadot.*cos(theta)/2 + I_2.^2.*psidot.*thetadot.*cos(2*phi - theta)/4 + I_2.^2.*psidot.*thetadot.*cos(2*phi + theta)/4 - I_2.*I_3.*phidot.*psidot.*cos(2*phi - theta)/4 + I_2.*I_3.*phidot.*psidot.*cos(2*phi + theta)/4 - I_2.*I_3.*phidot.*thetadot.*cos(2*phi)/2 + I_2.*I_3.*phidot.*thetadot/2 - I_2.*I_3.*psidot.^2.*cos(2*phi - 2*theta)/8 + I_2.*I_3.*psidot.^2.*cos(2*phi + 2*theta)/8 + I_2.*I_3.*psidot.*thetadot.*cos(theta)/2 - I_2.*I_3.*psidot.*thetadot.*cos(2*phi - theta)/4 - I_2.*I_3.*psidot.*thetadot.*cos(2*phi + theta)/4)./(I_1.*I_2.*sin(theta));\n\nend\n")]

















In [19]:



    


codegen(("eom1",sol_thdd[0]), 'Octave', argument_sequence=[th,thd,psi,psid,ph,phd,I1,I2,I3])



    


















    
Out[19]:








[('eom1.m',
  "function out1 = eom1(theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = (I_1.^2.*phidot.*psidot.*sin(phi).^2.*sin(theta) + I_1.^2.*phidot.*thetadot.*sin(2*phi)/2 + I_1.^2.*psidot.^2.*sin(phi).^2.*sin(theta).*cos(theta) + I_1.^2.*psidot.*thetadot.*(sin(2*phi - theta) + sin(2*phi + theta))/4 - I_1.*I_2.*phidot.*psidot.*sin(phi).^2.*sin(theta) - I_1.*I_2.*phidot.*psidot.*sin(theta).*cos(phi).^2 - I_1.*I_3.*phidot.*psidot.*sin(phi).^2.*sin(theta) - I_1.*I_3.*phidot.*thetadot.*sin(2*phi)/2 - I_1.*I_3.*psidot.^2.*sin(phi).^2.*sin(theta).*cos(theta) - I_1.*I_3.*psidot.*thetadot.*(sin(2*phi - theta) + sin(2*phi + theta))/4 + I_2.^2.*phidot.*psidot.*sin(theta).*cos(phi).^2 - I_2.^2.*phidot.*thetadot.*sin(2*phi)/2 + I_2.^2.*psidot.^2.*sin(theta).*cos(phi).^2.*cos(theta) - I_2.^2.*psidot.*thetadot.*(sin(2*phi - theta) + sin(2*phi + theta))/4 - I_2.*I_3.*phidot.*psidot.*sin(theta).*cos(phi).^2 + I_2.*I_3.*phidot.*thetadot.*sin(2*phi)/2 - I_2.*I_3.*psidot.^2.*sin(theta).*cos(phi).^2.*cos(theta) + I_2.*I_3.*psidot.*thetadot.*(sin(2*phi - theta) + sin(2*phi + theta))/4)./(I_1.*I_2);\n\nend\n")]

















In [20]:



    


codegen(("eom1",sol_phdd[0]), 'Octave', argument_sequence=[th,thd,psi,psid,ph,phd,I1,I2,I3,psidd])



    


















    
Out[20]:








[('eom1.m',
  "function out1 = eom1(theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3, psiddot)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = (I_1.*psidot.^2.*sin(phi).*sin(theta).^2.*cos(phi) - 2*I_1.*psidot.*thetadot.*sin(phi).^2.*sin(theta) + I_1.*psidot.*thetadot.*sin(theta) - I_1.*thetadot.^2.*sin(phi).*cos(phi) - I_2.*psidot.^2.*sin(phi).*sin(theta).^2.*cos(phi) + 2*I_2.*psidot.*thetadot.*sin(phi).^2.*sin(theta) - I_2.*psidot.*thetadot.*sin(theta) + I_2.*thetadot.^2.*sin(phi).*cos(phi) - I_3.*psiddot.*cos(theta) + I_3.*psidot.*thetadot.*sin(theta))./I_3;\n\nend\n")]

















In [21]:



    


codegen(("eom1",[s0[psid],s0[thd],s0[phd]]), 'Octave', argument_sequence=[w1,w2,w3,th,thd,psi,psid,ph,phd,I1,I2,I3,psidd])



    


















    
Out[21]:








[('eom1.m',
  "function [out1, out2, out3] = eom1(omega_1, omega_2, omega_3, theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3, psiddot)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = (omega_1.*sin(phi) + omega_2.*cos(phi))./sin(theta);\n  out2 = omega_1.*cos(phi) - omega_2.*sin(phi);\n  out3 = -omega_1.*sin(phi)./tan(theta) - omega_2.*cos(phi)./tan(theta) + omega_3;\n\nend\n")]

















In [22]:



    


codegen(("eom1",bCi), 'Octave', argument_sequence=[th,thd,psi,psid,ph,phd,I1,I2,I3,psidd])



    


















    
Out[22]:








[('eom1.m',
  "function out1 = eom1(theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3, psiddot)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = [-sin(phi).*sin(psi).*cos(theta) + cos(phi).*cos(psi) sin(phi).*cos(psi).*cos(theta) + sin(psi).*cos(phi) sin(phi).*sin(theta); -sin(phi).*cos(psi) - sin(psi).*cos(phi).*cos(theta) -sin(phi).*sin(psi) + cos(phi).*cos(psi).*cos(theta) sin(theta).*cos(phi); sin(psi).*sin(theta) -sin(theta).*cos(psi) cos(theta)];\n\nend\n")]

















In [23]:



    


codegen(("eom1",omega), 'Octave', argument_sequence=[w1,w2,w3,th,thd,psi,psid,ph,phd,I1,I2,I3,psidd])



    


















    
Out[23]:








[('eom1.m',
  "function out1 = eom1(omega_1, omega_2, omega_3, theta, thetadot, psi, psidot, phi, phidot, I_1, I_2, I_3, psiddot)\n  %EOM1  Autogenerated by sympy\n  %   Code generated with sympy 1.3\n  %\n  %   See http://www.sympy.org/ for more information.\n  %\n  %   This file is part of 'project'\n\n  out1 = [psidot.*sin(phi).*sin(theta) + thetadot.*cos(phi); psidot.*sin(theta).*cos(phi) - thetadot.*sin(phi); phidot + psidot.*cos(theta)];\n\nend\n")]

















In [ ]:



    






    











In [ ]:



    






    











In [ ]:



    






    











In [ ]:



    






    











In [ ]:



    






    











In [ ]:

Content source: dsavransky/MAE4060

Similar notebooks:

notebook.community | gallery | about